﻿using System;

namespace 历史积累
{
    public static class BasicAlgorithm
    {
        /// <summary>
        /// 波形曲线 算频率
        /// </summary>
        /// <param name="pData">波形曲线数据</param>
        /// <param name="smpRate">采样率</param>
        /// <returns></returns>
        public static double GetFrequency(double[] pData, int smpRate)
        {
            if (pData == null || smpRate < 400) return 0;
            double sum, deltAng, deltFreq, tempfreq = 125;
            double[] imag = new double[2], real = new double[2], angle = new double[2];
            int i, k, tempPCount = smpRate / 125;
            for (i = 0; i < 2; i++)
            {
                sum = 0;
                for (k = 0; k < tempPCount; k++)
                    sum += pData[i * tempPCount + k] * Math.Cos(2 * Math.PI * k / tempPCount);
                imag[i] = 2 * sum / tempPCount;//虚部
                sum = 0;
                for (k = 0; k < tempPCount; k++)
                    sum += pData[i * tempPCount + k] * Math.Sin(2 * Math.PI * k / tempPCount);
                real[i] = 2 * sum / tempPCount;//实部
                angle[i] = (imag[i] < 0) ? (2 * Math.PI) + Math.Atan2(imag[i], real[i]) : Math.Atan2(imag[i], real[i]);
            }
            //计算角差
            deltAng = angle[1] - angle[0];
            deltAng = 1 / (1 + ((deltAng > Math.PI) ? (deltAng - 2 * Math.PI) : ((deltAng < -Math.PI) ? (deltAng + 2 * Math.PI) : deltAng)) / (2 * Math.PI));
            //修正角差
            deltFreq = Math.Atan2(imag[1] * deltAng, real[1]) - Math.Atan2(imag[0] * deltAng, real[0]);
            deltFreq = (deltFreq > Math.PI) ? (deltFreq - 2 * Math.PI) : ((deltFreq < -Math.PI) ? (deltFreq + 2 * Math.PI) : deltFreq);
            tempfreq += tempfreq * deltFreq / (2 * Math.PI);
            // Console.WriteLine(tempfreq);
            return tempfreq;
        }
    }
}